﻿#include "include.h"
#include "gait_math.h"
#include "force_dis_8.h"
#if !RUN_WEBOTS||RUN_PI
#include "qp_fdis.h"
#else
#include "optimaize.h"
#endif
#include "locomotion_header.h"

float FLT_ATT_CTRL_Y = 0.35;
float FLT_TORQUE = 25;
float FLT_ATT_SPD_ERR = 6;
float k_att_spd = 25;
float att_trig_inner[2];
float k_att_inner_small[2] = { 0,0 };
float delay_rotate = 0.0;
void body_traj_planner(float dt)
{
	char i, j;
	float att_rc_off[3] = { 0 };
	float att_use[3];
	float delay_time = 0;
	float climb_cog_off[3] = { 0 };
	att_use[PITr] = vmc_all.att_vm_b[PITr];
	att_use[ROLr] = vmc_all.att_ctrl[ROLr];

	//--------------------
	if(vmc_all.gait_mode==TROT)
		SLIP_Traj_Model(dt);

#if !EN_HUMAN
	vmc_all.tar_spd.z = FORWARD_SPD_YAW;
#endif
	vmc_all.param.tar_spd_use_rc.x = vmc_all.tar_spd.x;// *cosd(vmc_all.ground_att_est[PITr]);
	vmc_all.param.tar_spd_use_rc.y = vmc_all.tar_spd.y;
	vmc_all.param.tar_spd_use_rc.z = vmc_all.tar_spd.z;
	//printf("vmc_all.param.tar_spd_use_rc.x=%f\n", vmc_all.param.tar_spd_use_rc.x);
	//--------------------
	//vmc_all.cog_off[2] = vmc_all.param.param_vmc.move_att_off[PITr];
	//vmc_all.tar_att_off[PITr] = LIMIT(vmc_all.param.tar_spd_use_rc.x, -MAX_SPD, MAX_SPD) / (MAX_SPD + 0.00001)*vmc_all.cog_off[2];
	
	//--------------------
	if (fabs(vmc_all.param.tar_spd_use_rc.x) > MAX_SPD*0.2&&fabs(vmc_all.tar_spd.z) > MAX_SPD_RAD*0.1) {
		if (vmc_all.param.tar_spd_use_rc.x > 0)
			att_rc_off[ROLr] = -vmc_all.param.param_vmc.move_att_off[ROLr] * sign(vmc_all.param.tar_spd_use_rc.x)*LIMIT(vmc_all.tar_spd.z, -MAX_SPD_RAD * 0.35, MAX_SPD_RAD*0.35) / (MAX_SPD_RAD*0.35 + 0.00001);
		else
			att_rc_off[ROLr] = -vmc_all.param.param_vmc.move_att_off[ROLr] * sign(vmc_all.param.tar_spd_use_rc.x)*LIMIT(vmc_all.tar_spd.z, -MAX_SPD_RAD * 0.35, MAX_SPD_RAD*0.35) / (MAX_SPD_RAD*0.35 + 0.00001);
	}
	DigitalLPF(att_rc_off[ROLr], &vmc_all.tar_att_off[ROLr], 0.8, dt);

	//----------------------height down for climb-----------------------------------
	climb_cog_off[Xr] = -sind(my_deathzoom_2((vmc_all.ground_att_est[PITr]), 8) * 2)*vmc_all.param.climb_off.x;// *sign(robotwb.exp_spd_n.x);
	climb_cog_off[Zr] = -sind(my_deathzoom_2(fabs(vmc_all.ground_att_est[PITr])+ fabs(vmc_all.ground_att_est[ROLr]), 8) * 2)*vmc_all.param.climb_off.z;
	DigitalLPF(climb_cog_off[Xr], &vmc_all.climb_off.x, 0.8, dt);
	DigitalLPF(climb_cog_off[Zr], &vmc_all.climb_off.z, 0.8, dt);
	//printf("vmc_all.climb_off.x=%f\n", vmc_all.climb_off.x);
	//printf("vmc_all.climb_off.z=%f\n", vmc_all.climb_off.z);
#if 0
	if (fabs(vmc_all.param.tar_spd_use_rc.x) > 0.02) {
		delay_time = vmc_all.delay_time[0];
		DigitalLPF(delay_time, &vmc_all.delay_time[1], 0.6, dt);
	}
	else {
		if (fabs(vmc_all.tar_spd.z) < MAX_SPD_RAD*0.1) {
			delay_time = delay_rotate;
		}
		else
			delay_time = delay_rotate / 2;
		DigitalLPF(delay_time, &vmc_all.delay_time[1], 0.1, dt);
	}
#else
	delay_time = vmc_all.delay_time[0];
	DigitalLPF(delay_time, &vmc_all.delay_time[1], 0.1, dt);
#endif

}



#if USE_FORCE_REAL_GROUND
float gain_tort[6] = { 1,1,1,1,1,1 };
#else
float gain_tort[6] = { 2,1,2,2,2,1 };
#endif
void body_servo_control(float dt)//机体伺服控制器
{
	char i;
	static float spdx_reg = 0, spdy_reg = 0;
	float att_cmd[3] = { 0 };
	float pos_cmd[3] = { 0 };
	float exp_rate_yaw = 0;
	float temp = 0;
	float exp_se3[3] = { 0 };
	float err_so3[3] = { 0 };
	//期望状态滤波
	if (vmc_all.gait_mode == PRONK || vmc_all.gait_mode == CLIMB) {
		DigitalLPF(vmc_all.tar_pos.x, &robotwb.exp_pos_n.x, 15, dt);
		DigitalLPF(vmc_all.tar_pos.y, &robotwb.exp_pos_n.y, 15, dt);
		DigitalLPF(vmc_all.tar_pos.z, &robotwb.exp_pos_n.z, 15, dt);
	}
	else {
		DigitalLPF(vmc_all.tar_pos.x, &robotwb.exp_pos_n.x, 1.5, dt);
		DigitalLPF(vmc_all.tar_pos.y, &robotwb.exp_pos_n.y, 1.5, dt);
		DigitalLPF(vmc_all.tar_pos.z, &robotwb.exp_pos_n.z, 1.5, dt);
	}
#if !EN_HUMAN
	vmc_all.tar_spd.z = FORWARD_SPD_YAW;
#endif
	exp_rate_yaw = vmc_all.tar_spd.z;

#if USE_MPC||USE_MPC_QP
	Vect3 temp_g, temp_n;

	temp_n.x = robotwb.exp_spd_n.x = vmc_all.tar_spd.x;
	temp_n.y = robotwb.exp_spd_n.y = vmc_all.tar_spd.y;
	temp_n.z = 0;
	converV_n_to_g(temp_n, &temp_g);
	robotwb.exp_spd_ng.x = temp_g.x;
	robotwb.exp_spd_ng.y = temp_g.y;

	ConvexMPCLocomotion_run(dt);
	
#endif
	switch (vmc_all.gait_mode) {
	case STAND_RC:case STAND_IMU:case STAND_PUSH:case PRONK:
		if (stand_force_enable_flag[4]) {
			exp_se3[PITr] = robotwb.exp_att.pitch;
			exp_se3[ROLr] = robotwb.exp_att.roll;
			cal_ero_so3(exp_se3, &err_so3[0]);
			//printf("ep=%f er=%f eso0=%f eso1=%f\n",
			//	robotwb.exp_att.pitch - robotwb.now_att.pitch,
			//	robotwb.exp_att.roll - robotwb.now_att.roll,
			//	err_so3[PITr],
			//	err_so3[ROLr]);
			//err_so3[PITr] = robotwb.exp_att.pitch - robotwb.now_att.pitch;
			//err_so3[ROLr] = robotwb.exp_att.roll - robotwb.now_att.roll;
			//Pitr-----------------------------------------
			if (vmc_all.ground_num >= 3)
				robotwb.exp_torque_i.y += limitw(err_so3[PITr], -15, 15) / 57.3*vmc_robot_p.att_pit.ki*dt;
			else if (vmc_all.ground_num <= 1)
				robotwb.exp_torque_i.y = 0;
			robotwb.exp_torque_i.y = limitw(robotwb.exp_torque_i.y, -robotwb.max_torque.y*0.35, robotwb.max_torque.y*0.35);

			robotwb.exp_torque.y = att_cmd[PITr] = limitw(err_so3[PITr], -15, 15) / 57.3*vmc_robot_p.att_pit.kp
				- robotwb.now_rate.pitch / 57.3*vmc_robot_p.att_pit.kd + robotwb.exp_torque_i.y;
			robotwb.exp_torque.y = limitw(robotwb.exp_torque.y, -robotwb.max_torque.y, robotwb.max_torque.y);

			//Rolr------------------------------------------
			if (vmc_all.ground_num >= 3)
				robotwb.exp_torque_i.x += limitw(err_so3[ROLr], -25, 25) / 57.3*vmc_robot_p.att_rol.ki*dt;
			else if (vmc_all.ground_num <= 1)
				robotwb.exp_torque_i.x = 0;

			robotwb.exp_torque_i.x = limitw(robotwb.exp_torque_i.x, -robotwb.max_torque.x*0.25, robotwb.max_torque.x*0.25);

			robotwb.exp_torque.x = att_cmd[ROLr] = limitw(err_so3[ROLr], -25, 25) / 57.3*vmc_robot_p.att_rol.kp
				- robotwb.now_rate.roll / 57.3*vmc_robot_p.att_rol.kd + robotwb.exp_torque_i.x;
			robotwb.exp_torque.x = limitw(robotwb.exp_torque.x, -robotwb.max_torque.x, robotwb.max_torque.x);

			robotwb.exp_torque.z = 0;

			//Xr
			if (vmc_all.ground_num >= 3)
				robotwb.exp_force_i.x += (robotwb.exp_pos_n.x - vmc_all.pos_n.x)*vmc_robot_p.pos_x.ki*dt;
			else if (vmc_all.ground_num <= 1)
				robotwb.exp_force_i.x = 0;

			robotwb.exp_force_i.x = limitw(robotwb.exp_force_i.x, -robotwb.max_force.x*0.25, robotwb.max_force.x*0.25);

			robotwb.exp_force.x = pos_cmd[Xr] = (robotwb.exp_pos_n.x - vmc_all.pos_n.x)*vmc_robot_p.pos_x.kp
				- vmc_all.spd_n.x*vmc_robot_p.pos_x.kd + robotwb.exp_force_i.x + robotwb.cog_F_ff.x;

			//Xr
			if (vmc_all.ground_num >= 3)
				robotwb.exp_force_i.y += (robotwb.exp_pos_n.y - vmc_all.pos_n.y)*vmc_robot_p.pos_y.ki*dt;
			else if (vmc_all.ground_num <= 1)
				robotwb.exp_force_i.y = 0;

			robotwb.exp_force_i.y = limitw(robotwb.exp_force_i.y, -robotwb.max_force.y*0.25, robotwb.max_force.y*0.25);

			robotwb.exp_force.y = pos_cmd[Yr] = (robotwb.exp_pos_n.y - vmc_all.pos_n.y)*vmc_robot_p.pos_y.kp
				- vmc_all.spd_n.y*vmc_robot_p.pos_y.kd + robotwb.exp_force_i.y + robotwb.cog_F_ff.y;

			//Zr
			if (vmc_all.ground_num >= 3)
				robotwb.exp_force_i.z += (fabs(robotwb.exp_pos_n.z) + vmc_all.climb_off.z - vmc_all.pos_n.z)*vmc_robot_p.pos_z.ki*dt;
			else if (vmc_all.ground_num <= 1)
				robotwb.exp_force_i.z = 0;
			robotwb.exp_force_i.z = limitw(robotwb.exp_force_i.z, -robotwb.max_force.z*0.25, robotwb.max_force.z*0.25);

			robotwb.exp_force.z = pos_cmd[Zr] = (fabs(robotwb.exp_pos_n.z) + vmc_all.climb_off.z - vmc_all.pos_n.z)*vmc_robot_p.pos_z.kp
				+ vmc_all.spd_n.z*vmc_robot_p.pos_z.kd + robotwb.exp_force_i.z;

			robotwb.exp_force.z += Mw * gw*vmc_robot_p.mess_scale + robotwb.cog_F_ff.z;//重力前馈
			//YAWr
			robotwb.exp_att.yaw = vmc_all.tar_att[YAWr];
			if (fabs(robotwb.exp_att.yaw - To_180_degrees(vmc_all.att_vm[YAWr] - robotwb.exp_att.yaw_off)) > 45)
				robotwb.exp_att.yaw = vmc_all.tar_att[YAWr] = To_180_degrees(vmc_all.att_vm[YAWr] - robotwb.exp_att.yaw_off);
			robotwb.exp_torque.z = limitw(robotwb.exp_att.yaw - To_180_degrees(vmc_all.att_vm[YAWr] - robotwb.exp_att.yaw_off), -25, 25) / 57.3*vmc_robot_p.att_yaw.kp
				- (robotwb.now_rate.yaw) / dt / 57.3*vmc_robot_p.att_yaw.kd;
			robotwb.exp_torque.z += robotwb.cog_T_ff.z;
			robotwb.exp_torque.z = limitw(robotwb.exp_torque.z, -robotwb.max_torque.z, robotwb.max_torque.z);
			//printf("exp=%f now=%f out=%f\n",robotwb.exp_att.yaw,To_180_degrees(vmc_all.att_vm[YAWr] -robotwb.exp_att.yaw_off),robotwb.exp_torque.z);
			force_dis_n();
		}
		else
		{//清除积分
			robotwb.exp_torque_i.x = robotwb.exp_torque_i.y = robotwb.exp_torque_i.z = 0;
			robotwb.exp_force_i.x = robotwb.exp_force_i.y = robotwb.exp_force_i.z = 0;

			robotwb.exp_force.x = robotwb.exp_force.y = robotwb.exp_force.z = 0;
			robotwb.exp_torque.x = robotwb.exp_torque.y = robotwb.exp_torque.z = 0;
		}
		robotwb.exp_att.yaw = robotwb.now_att.yaw;
		break;
		//----------------------------------------------------
		//----------------------------------------------------
		//----------------------------------------------------
	case TROT:case F_TROT:case G_ETL:case WALK:
		if (stand_force_enable_flag[4]) {
			exp_se3[PITr] = robotwb.exp_att.pitch + vmc_all.tar_att_off[PITr];
			exp_se3[ROLr] = robotwb.exp_att.roll;
			cal_ero_so3(exp_se3, &err_so3[0]);
			//printf("ep=%f er=%f eso0=%f eso1=%f\n",
			//	robotwb.exp_att.pitch - robotwb.now_att.pitch,
			//	robotwb.exp_att.roll - robotwb.now_att.roll,
			//	err_so3[PITr],
			//	err_so3[ROLr]);
#if 0
			err_so3[PITr] = robotwb.exp_att.pitch + vmc_all.tar_att_off[PITr] - robotwb.now_att.pitch;
			err_so3[ROLr] = robotwb.exp_att.roll - robotwb.now_att.roll;
#endif
			//Pitr-----------------------------------------
			if (vmc_all.ground_num >= 2)
				robotwb.exp_torque_i.y += limitw(err_so3[PITr], -35, 35) / 57.3*vmc_robot_p.att_pit_trot.ki*dt;

			robotwb.exp_torque_i.y = limitw(robotwb.exp_torque_i.y, -robotwb.max_torque.y*0.35, robotwb.max_torque.y*0.35);

			robotwb.exp_torque.y = att_cmd[PITr] = limitw(err_so3[PITr], -35, 35) / 57.3*vmc_robot_p.att_pit_trot.kp
				- robotwb.now_rate.pitch / 57.3*vmc_robot_p.att_pit_trot.kd + robotwb.exp_torque_i.y;
			robotwb.exp_torque.y = limitw(robotwb.exp_torque.y, -robotwb.max_torque.y, robotwb.max_torque.y);

			//Rolr------------------------------------------
#if RUN_PI
			if (vmc_all.param.robot_mode == M_TROT) {
				if (vmc_all.ground_num >= 4) {
					vmc_robot_p.att_rol_trot.kp = config_gait["climb_trot_param_gait"]["kp_rol"].as<float>();//66;//ROL姿态
					vmc_robot_p.att_rol_trot.ki = config_gait["climb_trot_param_gait"]["ki_rol"].as<float>();//20.0;
					vmc_robot_p.att_rol_trot.kd = config_gait["climb_trot_param_gait"]["kd_rol"].as<float>();//1.5;
				}
				else {
					vmc_robot_p.att_rol_trot.kp = config_gait["trot_param"]["kp_rol"].as<float>();//66;//ROL姿态
					vmc_robot_p.att_rol_trot.ki = config_gait["trot_param"]["ki_rol"].as<float>();//20.0;
					vmc_robot_p.att_rol_trot.kd = config_gait["trot_param"]["kd_rol"].as<float>();//1.5;
				}
			}
			else {
				vmc_robot_p.att_rol_trot.kp = config_gait["trot_param"]["kp_rol"].as<float>();//66;//ROL姿态
				vmc_robot_p.att_rol_trot.ki = config_gait["trot_param"]["ki_rol"].as<float>();//20.0;
				vmc_robot_p.att_rol_trot.kd = config_gait["trot_param"]["kd_rol"].as<float>();//1.5;
			}
#endif

			if (vmc_all.ground_num >= 2)
				robotwb.exp_torque_i.x += limitw(err_so3[ROLr], -35, 35) / 57.3*vmc_robot_p.att_rol_trot.ki*dt;


			robotwb.exp_torque_i.x = limitw(robotwb.exp_torque_i.x, -robotwb.max_torque.x*0.35, robotwb.max_torque.x*0.35);

			robotwb.exp_torque.x = att_cmd[ROLr] = limitw(err_so3[ROLr], -35, 35) / 57.3*vmc_robot_p.att_rol_trot.kp
				- robotwb.now_rate.roll / 57.3*vmc_robot_p.att_rol_trot.kd + robotwb.exp_torque_i.x;
			robotwb.exp_torque.x = limitw(robotwb.exp_torque.x, -robotwb.max_torque.x, robotwb.max_torque.x);


			//YAWr------------------------------------------
 
			if (fabs(exp_rate_yaw) > 1)
			{
				robotwb.exp_rate.yaw = exp_rate_yaw;
				robotwb.exp_att.yaw = robotwb.now_att.yaw;
			}
			else {
				robotwb.exp_rate.yaw = dead(limitw(To_180_degrees(robotwb.exp_att.yaw - robotwb.now_att.yaw), -25, 25), 0.25)*vmc_robot_p.att_yaw_trot.kp_o;
			}
			//printf("robotwb.exp_rate.yaw=%f robotwb.exp_att.yaw=%f robotwb.now_att.yaw=%f\n", robotwb.exp_rate.yaw,robotwb.exp_att.yaw ,robotwb.now_att.yaw );
			if (vmc_all.ground_num >= 2)
				robotwb.exp_torque_i.z += limitw(robotwb.exp_rate.yaw - robotwb.now_rate.yaw, -15, 15) / 57.3*vmc_robot_p.att_yaw_trot.ki*dt;

			robotwb.exp_torque_i.z = limitw(robotwb.exp_torque_i.z, -robotwb.max_torque.z*0.2, robotwb.max_torque.z*0.2);

			robotwb.exp_torque.z = att_cmd[YAWr] = limitw(robotwb.exp_rate.yaw - robotwb.now_rate.yaw, -15, 15) / 57.3*vmc_robot_p.att_yaw_trot.kp
				- (robotwb.now_rate.yaw - robotwb.now_rate_reg.yaw) / dt / 57.3*vmc_robot_p.att_yaw_trot.kd
				+ robotwb.exp_torque_i.z
				+ robotwb.exp_rate.yaw / 57.3*vmc_robot_p.att_yaw_trot.vff;
			robotwb.exp_torque.z = limitw(robotwb.exp_torque.z, -robotwb.max_torque.z, robotwb.max_torque.z);
			//robotwb.exp_torque.z=0;

			if (vmc_all.gait_mode == WALK) {
				robotwb.exp_spd_n.x = vmc_all.tar_spd_walk[Xr];
				robotwb.exp_spd_n.y = vmc_all.tar_spd_walk[Yr];
			}
			else {
				robotwb.exp_spd_n.x = vmc_all.tar_spd.x;
				robotwb.exp_spd_n.y = vmc_all.tar_spd.y;
			}
			Vect3 temp_g, temp_n;
			temp_n.x = robotwb.exp_spd_n.x;
			temp_n.y = robotwb.exp_spd_n.y;
			temp_n.z = 0;
			converV_n_to_g(temp_n, &temp_g);
			robotwb.exp_spd_ng.x = temp_g.x;
			robotwb.exp_spd_ng.y = temp_g.y;
			//Xr		
			if (vmc_all.ground_num >= 2)
				robotwb.exp_force_i.x += (robotwb.exp_spd_ng.x - vmc_all.spd_ng.x)*vmc_robot_p.spd_x.ki*dt;
			else
				robotwb.exp_force_i.x = 0;

			if (!vmc_robot_p.spd_x.ki)robotwb.exp_force_i.x = 0;
			robotwb.exp_force_i.x = limitw(robotwb.exp_force_i.x, -robotwb.max_force.x*0.2, robotwb.max_force.x*0.2);

			robotwb.exp_force.x = pos_cmd[Xr] = (robotwb.exp_spd_ng.x - vmc_all.spd_ng.x)*vmc_robot_p.spd_x.kp + robotwb.exp_force_i.x
				- (vmc_all.spd_ng.x - spdx_reg) / dt * vmc_robot_p.spd_x.kd;
			//Yr
			if (vmc_all.ground_num >= 2)
				robotwb.exp_force_i.y += (robotwb.exp_spd_ng.y - vmc_all.spd_ng.y)*vmc_robot_p.spd_y.ki*dt;
			else
				robotwb.exp_force_i.y = 0;

			if (!vmc_robot_p.spd_y.ki)robotwb.exp_force_i.y = 0;
			robotwb.exp_force_i.y = limitw(robotwb.exp_force_i.y, -robotwb.max_force.y*0.2, robotwb.max_force.y*0.2);

			robotwb.exp_force.y = pos_cmd[Yr] = (robotwb.exp_spd_ng.y - vmc_all.spd_ng.y)*vmc_robot_p.spd_y.kp + robotwb.exp_force_i.y
				- (vmc_all.spd_ng.y - spdy_reg) / dt * vmc_robot_p.spd_y.kd;

			//Zr
			if (vmc_all.ground_num >= 2)
				robotwb.exp_force_i.z += (fabs(robotwb.exp_pos_n.z) + vmc_all.climb_off.z - vmc_all.pos_n.z)*vmc_robot_p.pos_z_trot.ki*dt;

			robotwb.exp_force_i.z = limitw(robotwb.exp_force_i.z, -robotwb.max_force.z*0.3, robotwb.max_force.z*0.3);

			robotwb.exp_force.z = pos_cmd[Zr] = (fabs(robotwb.exp_pos_n.z) + vmc_all.climb_off.z - vmc_all.pos_n.z)*vmc_robot_p.pos_z_trot.kp
				+ vmc_all.spd_n.z*vmc_robot_p.pos_z_trot.kd + robotwb.exp_force_i.z;

			//robotwb.exp_torque.x=robotwb.exp_torque.y=robotwb.exp_torque.z=0;//测试用屏蔽姿态
			//printf("set=%f now_z=%f spd=%f\n", fabs(robotwb.exp_pos_n.z) + vmc_all.climb_off.z, vmc_all.pos_n.z, vmc_all.spd_n.z);
			robotwb.exp_force.x = (pos_cmd[Xr] + slip_mode.slip_F[Xr] + 0 * 0.5*Mw*gw*limitw(sind(vmc_all.ground_att_est[PITr]), -0.35, 0.35)*vmc_robot_p.mess_scale);//地形重力补偿
			robotwb.exp_force.y = (pos_cmd[Yr] + slip_mode.slip_F[Yr]);
			robotwb.exp_force.z = (pos_cmd[Zr] + slip_mode.slip_F[Zr] + Mw * gw*vmc_robot_p.mess_scale * (vmc_all.gait_mode == TROT));//FTROT
			robotwb.exp_torque.y = (att_cmd[PITr]);
			robotwb.exp_torque.x = (att_cmd[ROLr]);
			robotwb.exp_torque.z = (att_cmd[YAWr]);

			force_dis_n();
		}
		else
		{//清除积分
			robotwb.exp_torque_i.x = robotwb.exp_torque_i.y = robotwb.exp_torque_i.z = 0;
			robotwb.exp_force_i.x = robotwb.exp_force_i.y = robotwb.exp_force_i.z = 0;

			robotwb.exp_force.x = robotwb.exp_force.y = robotwb.exp_force.z = 0;
			robotwb.exp_torque.x = robotwb.exp_torque.y = robotwb.exp_torque.z = 0;
		}
		break;
		//----------------------------------------------------
		//----------------------------------------------------
		//----------------------------------------------------
//	case WALK:
//		if (stand_force_enable_flag[4]) {
//			//Pitr-----------------------------------------
//			if (vmc_all.ground_num >= 2)
//				robotwb.exp_torque_i.y += limitw(robotwb.exp_att.pitch + vmc_all.tar_att_off[PITr] - robotwb.now_att.pitch, -35, 35) / 57.3*vmc_robot_p.att_pit_walk.ki*dt;
//
//			robotwb.exp_torque_i.y = limitw(robotwb.exp_torque_i.y, -robotwb.max_torque.y*0.35, robotwb.max_torque.y*0.35);
//
//			robotwb.exp_torque.y = att_cmd[PITr] = limitw(robotwb.exp_att.pitch + vmc_all.tar_att_off[PITr] - robotwb.now_att.pitch, -35, 35) / 57.3*vmc_robot_p.att_pit_walk.kp
//				- robotwb.now_rate.pitch / 57.3*vmc_robot_p.att_pit_walk.kd + robotwb.exp_torque_i.y;
//			robotwb.exp_torque.y = limitw(robotwb.exp_torque.y, -robotwb.max_torque.y, robotwb.max_torque.y);
//
//			//Rolr------------------------------------------
//			if (vmc_all.ground_num >= 2)
//				robotwb.exp_torque_i.x += limitw(robotwb.exp_att.roll - robotwb.now_att.roll, -35, 35) / 57.3*vmc_robot_p.att_rol_walk.ki*dt;
//
//
//			robotwb.exp_torque_i.x = limitw(robotwb.exp_torque_i.x, -robotwb.max_torque.x*0.35, robotwb.max_torque.x*0.35);
//
//			robotwb.exp_torque.x = att_cmd[ROLr] = limitw(robotwb.exp_att.roll - robotwb.now_att.roll, -35, 35) / 57.3*vmc_robot_p.att_rol_walk.kp
//				- robotwb.now_rate.roll / 57.3*vmc_robot_p.att_rol_walk.kd + robotwb.exp_torque_i.x;
//			robotwb.exp_torque.x = limitw(robotwb.exp_torque.x, -robotwb.max_torque.x, robotwb.max_torque.x);
//
//
//			//YAWr------------------------------------------
//			if (fabs(exp_rate_yaw) > 1)
//			{
//				robotwb.exp_rate.yaw = exp_rate_yaw;
//				robotwb.exp_att.yaw = robotwb.now_att.yaw;
//			}
//			else {
//				robotwb.exp_rate.yaw = -dead(limitw(To_180_degrees(robotwb.exp_att.yaw - robotwb.now_att.yaw), -25, 25), 0.5)*vmc_robot_p.att_yaw_trot.kp_o;
//			}
//
//			if (vmc_all.ground_num >= 2)
//				robotwb.exp_torque_i.z += limitw(robotwb.exp_rate.yaw - robotwb.now_rate.yaw, -15, 15) / 57.3*vmc_robot_p.att_yaw_trot.ki*dt;
//
//			robotwb.exp_torque_i.z = limitw(robotwb.exp_torque_i.z, -robotwb.max_torque.z*0.2, robotwb.max_torque.z*0.2);
//
//			robotwb.exp_torque.z = att_cmd[YAWr] = limitw(robotwb.exp_rate.yaw - robotwb.now_rate.yaw, -15, 15) / 57.3*vmc_robot_p.att_yaw_trot.kp
//				- (robotwb.now_rate.yaw - robotwb.now_rate_reg.yaw) / dt / 57.3*vmc_robot_p.att_yaw_trot.kd
//				+ robotwb.exp_torque_i.z
//				+ robotwb.exp_rate.yaw / 57.3*vmc_robot_p.att_yaw_trot.vff;
//			robotwb.exp_torque.z = limitw(robotwb.exp_torque.z, -robotwb.max_torque.z, robotwb.max_torque.z);
//			//robotwb.exp_torque.z=0;
//
//			//Xr
//#if USE_ZMP
//			if (walk_gait.cog_stable)
//				temp = vmc_all.tar_spd.x;
//			else
//				temp = vmc_all.tar_spd.x*0.6;
//			if (walk_gait.area_too_small)temp = 0;
//#else
//			temp = vmc_all.tar_spd.x;
//#endif
//
//			DigitalLPF(temp, &robotwb.exp_spd_n.x, 6, dt);
//
//			if (vmc_all.ground_num >= 2)
//				robotwb.exp_force_i.x += (robotwb.exp_spd_n.x - vmc_all.spd_n.x)*vmc_robot_p.spd_x_walk.ki*dt;
//			else
//				robotwb.exp_force_i.x = 0;
//
//			if (!vmc_robot_p.spd_x_walk.ki)robotwb.exp_force_i.x = 0;
//			robotwb.exp_force_i.x = limitw(robotwb.exp_force_i.x, -robotwb.max_force.x*0.35, robotwb.max_force.x*0.35);
//
//
//			robotwb.exp_force.x = pos_cmd[Xr] = (robotwb.exp_spd_n.x
//				- vmc_all.spd_n.x)*vmc_robot_p.spd_x_walk.kp + robotwb.exp_force_i.x
//				- (vmc_all.spd_n.x - spdx_reg) / dt * vmc_robot_p.spd_x_walk.kd +
//				(robotwb.exp_pos_n.x - vmc_all.pos_n.x)*vmc_robot_p.pos_x_walk.kp*USE_ZMP;
//			//printf("robotwb.exp_pos_n.x=%f %f \n", robotwb.exp_pos_n.x , vmc_all.pos_n.x);
//
//			//Zr
//			if (vmc_all.ground_num >= 2)
//				robotwb.exp_force_i.z += (fabs(robotwb.exp_pos_n.z) + vmc_all.climb_off.z - vmc_all.pos_n.z)*vmc_robot_p.pos_z_walk.ki*dt;
//
//			robotwb.exp_force_i.z = limitw(robotwb.exp_force_i.z, -robotwb.max_force.z*0.3, robotwb.max_force.z*0.3);
//
//			robotwb.exp_force.z = pos_cmd[Zr] = (fabs(robotwb.exp_pos_n.z) + vmc_all.climb_off.z - vmc_all.pos_n.z)*vmc_robot_p.pos_z_walk.kp
//				+ vmc_all.spd_n.z*vmc_robot_p.pos_z_walk.kd + robotwb.exp_force_i.z;
//
//			//robotwb.exp_torque.x=robotwb.exp_torque.y=robotwb.exp_torque.z=0;//测试用屏蔽姿态
//
//			robotwb.exp_force.x = (pos_cmd[Xr] + slip_mode.slip_F[Xr] + 0 * 0.5*Mw*gw*limitw(sind(vmc_all.ground_att_est[PITr]), -0.35, 0.35)*vmc_robot_p.mess_scale);//地形重力补偿
//			robotwb.exp_force.y = (pos_cmd[Yr]);
//			robotwb.exp_force.z = (pos_cmd[Zr] + slip_mode.slip_F[Zr] + Mw * gw*vmc_robot_p.mess_scale);
//			robotwb.exp_torque.y = (att_cmd[PITr]);
//			robotwb.exp_torque.x = (att_cmd[ROLr]);
//			robotwb.exp_torque.z = (att_cmd[YAWr]);
//
//			force_dis_n();
//		}
//		else
//		{//清除积分
//			robotwb.exp_torque_i.x = robotwb.exp_torque_i.y = robotwb.exp_torque_i.z = 0;
//			robotwb.exp_force_i.x = robotwb.exp_force_i.y = robotwb.exp_force_i.z = 0;
//
//			robotwb.exp_force.x = robotwb.exp_force.y = robotwb.exp_force.z = 0;
//			robotwb.exp_torque.x = robotwb.exp_torque.y = robotwb.exp_torque.z = 0;
//		}
//		break;
		//----------------------------------------------------
		//----------------------------------------------------
		//----------------------------------------------------
	case BOUND://----------------------------------------------------

		if (stand_force_enable_flag[4]) {
			if (gait_bound.force_control_mode == 0) {
				//Pitr-----------------------------------------
				if (vmc_all.ground_num >= 3)
					robotwb.exp_torque_i.y += limitw(robotwb.exp_att.pitch - robotwb.now_att.pitch, -15, 15) / 57.3*vmc_robot_p.att_pit.ki*dt;
				else if (vmc_all.ground_num <= 1)
					robotwb.exp_torque_i.y = 0;
				robotwb.exp_torque_i.y = limitw(robotwb.exp_torque_i.y, -robotwb.max_torque.y*0.35, robotwb.max_torque.y*0.35);

				robotwb.exp_torque.y = att_cmd[PITr] = limitw(robotwb.exp_att.pitch - robotwb.now_att.pitch, -15, 15) / 57.3*vmc_robot_p.att_pit.kp
					- robotwb.now_rate.pitch / 57.3*vmc_robot_p.att_pit.kd + robotwb.exp_torque_i.y;
				robotwb.exp_torque.y = limitw(robotwb.exp_torque.y, -robotwb.max_torque.y, robotwb.max_torque.y);

				//Rolr------------------------------------------
				if (vmc_all.ground_num >= 3)
					robotwb.exp_torque_i.x += limitw(robotwb.exp_att.roll - robotwb.now_att.roll, -25, 25) / 57.3*vmc_robot_p.att_rol.ki*dt;
				else if (vmc_all.ground_num <= 1)
					robotwb.exp_torque_i.x = 0;

				robotwb.exp_torque_i.x = limitw(robotwb.exp_torque_i.x, -robotwb.max_torque.x*0.25, robotwb.max_torque.x*0.25);

				robotwb.exp_torque.x = att_cmd[ROLr] = limitw(robotwb.exp_att.roll - robotwb.now_att.roll, -25, 25) / 57.3*vmc_robot_p.att_rol.kp
					- robotwb.now_rate.roll / 57.3*vmc_robot_p.att_rol.kd + robotwb.exp_torque_i.x;
				robotwb.exp_torque.x = limitw(robotwb.exp_torque.x, -robotwb.max_torque.x, robotwb.max_torque.x);

				robotwb.exp_torque.z = 0;

				//Xr
				if (vmc_all.ground_num >= 3)
					robotwb.exp_force_i.x += (robotwb.exp_pos_n.x - vmc_all.pos_n.x)*vmc_robot_p.pos_x.ki*dt;
				else if (vmc_all.ground_num <= 1)
					robotwb.exp_force_i.x = 0;

				robotwb.exp_force_i.x = limitw(robotwb.exp_force_i.x, -robotwb.max_force.x*0.25, robotwb.max_force.x*0.25);

				robotwb.exp_force.x = pos_cmd[Xr] = (robotwb.exp_pos_n.x - vmc_all.pos_n.x)*vmc_robot_p.pos_x.kp
					- vmc_all.spd_n.x*vmc_robot_p.pos_x.kd + robotwb.exp_force_i.x + robotwb.cog_F_ff.x;

				//Zr
				if (vmc_all.ground_num >= 3)
					robotwb.exp_force_i.z += (fabs(robotwb.exp_pos_n.z) - vmc_all.pos_n.z)*vmc_robot_p.pos_z.ki*dt;
				else if (vmc_all.ground_num <= 1)
					robotwb.exp_force_i.z = 0;
				robotwb.exp_force_i.z = limitw(robotwb.exp_force_i.z, -robotwb.max_force.z*0.25, robotwb.max_force.z*0.25);

				robotwb.exp_force.z = pos_cmd[Zr] = (fabs(robotwb.exp_pos_n.z) - vmc_all.pos_n.z)*vmc_robot_p.pos_z.kp
					+ vmc_all.spd_n.z*vmc_robot_p.pos_z.kd + robotwb.exp_force_i.z;

				robotwb.exp_force.z += Mw * gw*vmc_robot_p.mess_scale + robotwb.cog_F_ff.z;//重力前馈

				robotwb.exp_torque.z = robotwb.cog_T_ff.z;
				//robotwb.exp_torque.x=robotwb.exp_torque.y=robotwb.exp_torque.z=0;//测试用屏蔽姿态
				force_dis_n();
			}
			else {//前后腿独立控制模式
				if (gait_bound.force_control_mode == 2)
				{

				}
				else
				{
					//Xr
					for (i = 0; i < 2; i++) {
						robotwb.exp_force_i_b[i].x += (robotwb.exp_pos_n_b[i].x - vmc_all.pos_n_b[i].x)*vmc_robot_p.pos_x.ki*dt;

						robotwb.exp_force_i_b[i].x = limitw(robotwb.exp_force_i_b[i].x, -robotwb.max_force.x*0.25, robotwb.max_force.x*0.25);

						robotwb.exp_force_b[i].x = (robotwb.exp_pos_n_b[i].x - vmc_all.pos_n_b[i].x)*vmc_robot_p.pos_x.kp
							- vmc_all.spd_n_b[i].x*vmc_robot_p.pos_x.kd + robotwb.exp_force_i_b[i].x;
						robotwb.exp_force_b[i].x *= 0.5;
					}
					for (i = 0; i < 2; i++) {
						//Zr
						robotwb.exp_force_i_b[i].z += (fabs(robotwb.exp_pos_n_b[i].z) - vmc_all.pos_n_b[i].z)*vmc_robot_p.pos_z.ki*dt;

						robotwb.exp_force_i_b[i].z = limitw(robotwb.exp_force_i_b[i].z, -robotwb.max_force.z*0.25, robotwb.max_force.z*0.25);

						robotwb.exp_force_b[i].z = (fabs(robotwb.exp_pos_n_b[i].z) - vmc_all.pos_n_b[i].z)*vmc_robot_p.pos_z.kp
							+ vmc_all.spd_n_b[i].z*vmc_robot_p.pos_z.kd + robotwb.exp_force_i_b[i].z;
						robotwb.exp_force_b[i].z *= 0.5;
						if (vmc_all.ground_num >= 3)
							robotwb.exp_force_b[i].z += Mw * gw*vmc_robot_p.mess_scale / 2;//重力前馈
						else
							robotwb.exp_force_b[i].z += Mw * gw*vmc_robot_p.mess_scale;//重力前馈
					}
				}
				//Rolr------------------------------------------
				if (vmc_all.ground_num >= 3)
					robotwb.exp_torque_i.x += limitw(robotwb.exp_att.roll - robotwb.now_att.roll, -25, 25) / 57.3*vmc_robot_p.att_rol_bound.ki*dt;
				else if (vmc_all.ground_num <= 1)
					robotwb.exp_torque_i.x = 0;

				robotwb.exp_torque_i.x = limitw(robotwb.exp_torque_i.x, -robotwb.max_torque.x*0.25, robotwb.max_torque.x*0.25);

				robotwb.exp_torque.x = att_cmd[ROLr] = limitw(robotwb.exp_att.roll - robotwb.now_att.roll, -25, 25) / 57.3*vmc_robot_p.att_rol_bound.kp
					- robotwb.now_rate.roll / 57.3*vmc_robot_p.att_rol_bound.kd + robotwb.exp_torque_i.x;
				robotwb.exp_torque.x = limitw(robotwb.exp_torque.x, -robotwb.max_torque.x, robotwb.max_torque.x);
				//printf("robotwb.exp_att.roll =%f\n",robotwb.exp_att.roll );
				//YAWr------------------------------------------

				robotwb.exp_rate.yaw = exp_rate_yaw;
				robotwb.exp_att.yaw = robotwb.now_att.yaw;

				if (vmc_all.ground_num >= 2)
					robotwb.exp_torque_i.z += limitw(robotwb.exp_rate.yaw - robotwb.now_rate.yaw, -15, 15) / 57.3*vmc_robot_p.att_yaw_bound.ki*dt;
				else
					robotwb.exp_torque_i.z = 0;

				robotwb.exp_torque_i.z = limitw(robotwb.exp_torque_i.z, -robotwb.max_torque.z*0.4, robotwb.max_torque.z*0.4);

				robotwb.exp_torque.z = att_cmd[YAWr] = limitw(robotwb.exp_rate.yaw - robotwb.now_rate.yaw, -55, 55) / 57.3*vmc_robot_p.att_yaw_bound.kp
					- (robotwb.now_rate.yaw - robotwb.now_rate_reg.yaw) / dt / 57.3*vmc_robot_p.att_yaw_bound.kd
					+ robotwb.exp_torque_i.z
					+ robotwb.exp_rate.yaw / 57.3*vmc_robot_p.att_yaw_bound.vff;
				robotwb.exp_torque.z = limitw(robotwb.exp_torque.z, -robotwb.max_torque.z, robotwb.max_torque.z);
				// robotwb.exp_torque.z=0;
				 //printf("")
					//printf("exp_z=%f %f\n", fabs(robotwb.exp_pos_n_b[F].z), vmc_all.pos_n_b[F].z);
				force_dis_n_side();
			}
		}
		else
		{//清除积分
			robotwb.exp_att.yaw = robotwb.now_att.yaw;
			robotwb.exp_torque_i.x = robotwb.exp_torque_i.y = robotwb.exp_torque_i.z = 0;
			robotwb.exp_force_i.x = robotwb.exp_force_i.y = robotwb.exp_force_i.z = 0;

			robotwb.exp_force.x = robotwb.exp_force.y = robotwb.exp_force.z = 0;
			robotwb.exp_torque.x = robotwb.exp_torque.y = robotwb.exp_torque.z = 0;
		}

		break;

		case ROLLING://----------------------------------------------------

			if (stand_force_enable_flag[4]) {		
				//Pitr-----------------------------------------
				if (vmc_all.ground_num >= 3)
					robotwb.exp_torque_i.y += limitw(robotwb.exp_att.pitch - robotwb.now_att.pitch, -15, 15) / 57.3*vmc_robot_p.att_pit.ki*dt;
				else if (vmc_all.ground_num <= 1)
					robotwb.exp_torque_i.y = 0;
				robotwb.exp_torque_i.y = limitw(robotwb.exp_torque_i.y, -robotwb.max_torque.y*0.35, robotwb.max_torque.y*0.35);

				robotwb.exp_torque.y = att_cmd[PITr] = limitw(robotwb.exp_att.pitch - robotwb.now_att.pitch, -15, 15) / 57.3*vmc_robot_p.att_pit.kp
					- robotwb.now_rate.pitch / 57.3*vmc_robot_p.att_pit.kd + robotwb.exp_torque_i.y;
				robotwb.exp_torque.y = limitw(robotwb.exp_torque.y, -robotwb.max_torque.y, robotwb.max_torque.y);

	
				robotwb.exp_torque.z=0;

				force_dis_n_side_LR();			
			}
			else
			{//清除积分
				robotwb.exp_att.yaw = robotwb.now_att.yaw;
				robotwb.exp_torque_i.x = robotwb.exp_torque_i.y = robotwb.exp_torque_i.z = 0;
				robotwb.exp_force_i.x = robotwb.exp_force_i.y = robotwb.exp_force_i.z = 0;

				robotwb.exp_force.x = robotwb.exp_force.y = robotwb.exp_force.z = 0;
				robotwb.exp_torque.x = robotwb.exp_torque.y = robotwb.exp_torque.z = 0;
			}

			break;
		//----------------------------------------------------
		//----------------------------------------------------
		//----------------------------------------------------
	case CLIMB://----------------------------------------------------------------------------------
		if (stand_force_enable_flag[4]) {
			if (gait_climb.force_control_mode == 0) {
				//Pitr-----------------------------------------
				if (vmc_all.ground_num >= 3)
					robotwb.exp_torque_i.y += limitw(robotwb.exp_att.pitch - robotwb.now_att.pitch, -15, 15) / 57.3*vmc_robot_p.att_pit.ki*dt;
				else if (vmc_all.ground_num <= 1)
					robotwb.exp_torque_i.y = 0;
				robotwb.exp_torque_i.y = limitw(robotwb.exp_torque_i.y, -robotwb.max_torque.y*0.35, robotwb.max_torque.y*0.35);

				robotwb.exp_torque.y = att_cmd[PITr] = limitw(robotwb.exp_att.pitch - robotwb.now_att.pitch, -15, 15) / 57.3*vmc_robot_p.att_pit.kp
					- robotwb.now_rate.pitch / 57.3*vmc_robot_p.att_pit.kd + robotwb.exp_torque_i.y;
				robotwb.exp_torque.y = limitw(robotwb.exp_torque.y, -robotwb.max_torque.y, robotwb.max_torque.y);

				//Rolr------------------------------------------
				if (vmc_all.ground_num >= 3)
					robotwb.exp_torque_i.x += limitw(robotwb.exp_att.roll - robotwb.now_att.roll, -25, 25) / 57.3*vmc_robot_p.att_rol.ki*dt;
				else if (vmc_all.ground_num <= 1)
					robotwb.exp_torque_i.x = 0;

				robotwb.exp_torque_i.x = limitw(robotwb.exp_torque_i.x, -robotwb.max_torque.x*0.25, robotwb.max_torque.x*0.25);

				robotwb.exp_torque.x = att_cmd[ROLr] = limitw(robotwb.exp_att.roll - robotwb.now_att.roll, -25, 25) / 57.3*vmc_robot_p.att_rol.kp
					- robotwb.now_rate.roll / 57.3*vmc_robot_p.att_rol.kd + robotwb.exp_torque_i.x;
				robotwb.exp_torque.x = limitw(robotwb.exp_torque.x, -robotwb.max_torque.x, robotwb.max_torque.x);

				robotwb.exp_torque.z = 0;

				//Xr
				if (vmc_all.ground_num >= 3)
					robotwb.exp_force_i.x += (robotwb.exp_pos_n.x - vmc_all.pos_n.x)*vmc_robot_p.pos_x.ki*dt;
				else if (vmc_all.ground_num <= 1)
					robotwb.exp_force_i.x = 0;

				robotwb.exp_force_i.x = limitw(robotwb.exp_force_i.x, -robotwb.max_force.x*0.25, robotwb.max_force.x*0.25);

				robotwb.exp_force.x = pos_cmd[Xr] = (robotwb.exp_pos_n.x - vmc_all.pos_n.x)*vmc_robot_p.pos_x.kp
					- vmc_all.spd_n.x*vmc_robot_p.pos_x.kd + robotwb.exp_force_i.x + robotwb.cog_F_ff.x;

				//Zr
				if (vmc_all.ground_num >= 3)
					robotwb.exp_force_i.z += (fabs(robotwb.exp_pos_n.z) - vmc_all.pos_n.z)*vmc_robot_p.pos_z.ki*dt;
				else if (vmc_all.ground_num <= 1)
					robotwb.exp_force_i.z = 0;
				robotwb.exp_force_i.z = limitw(robotwb.exp_force_i.z, -robotwb.max_force.z*0.25, robotwb.max_force.z*0.25);

				robotwb.exp_force.z = pos_cmd[Zr] = (fabs(robotwb.exp_pos_n.z) - vmc_all.pos_n.z)*vmc_robot_p.pos_z.kp
					+ vmc_all.spd_n.z*vmc_robot_p.pos_z.kd + robotwb.exp_force_i.z;

				robotwb.exp_force.z += Mw * gw*vmc_robot_p.mess_scale + robotwb.cog_F_ff.z;//重力前馈

				robotwb.exp_torque.z = robotwb.cog_T_ff.z;
				//robotwb.exp_torque.x=robotwb.exp_torque.y=robotwb.exp_torque.z=0;//测试用屏蔽姿态
				force_dis_n();
			}
			else {//前后腿独立控制模式

				//Xr
				for (i = 0; i < 2; i++) {
					robotwb.exp_force_i_b[i].x += (robotwb.exp_pos_n_b[i].x - vmc_all.pos_n_b[i].x)*vmc_robot_p.pos_x.ki*dt;

					robotwb.exp_force_i_b[i].x = limitw(robotwb.exp_force_i_b[i].x, -robotwb.max_force.x*0.25, robotwb.max_force.x*0.25);

					robotwb.exp_force_b[i].x = (robotwb.exp_pos_n_b[i].x - vmc_all.pos_n_b[i].x)*vmc_robot_p.pos_x.kp
						- vmc_all.spd_n_b[i].x*vmc_robot_p.pos_x.kd + robotwb.exp_force_i_b[i].x;
					robotwb.exp_force_b[i].x *= 0.5;
				}
				for (i = 0; i < 2; i++) {
					//Zr
					robotwb.exp_force_i_b[i].z += (fabs(robotwb.exp_pos_n_b[i].z) - vmc_all.pos_n_b[i].z)*vmc_robot_p.pos_z.ki*dt;

					robotwb.exp_force_i_b[i].z = limitw(robotwb.exp_force_i_b[i].z, -robotwb.max_force.z*0.25, robotwb.max_force.z*0.25);

					robotwb.exp_force_b[i].z = (fabs(robotwb.exp_pos_n_b[i].z) - vmc_all.pos_n_b[i].z)*vmc_robot_p.pos_z.kp
						+ vmc_all.spd_n_b[i].z*vmc_robot_p.pos_z.kd + robotwb.exp_force_i_b[i].z;
					robotwb.exp_force_b[i].z *= 0.5;
					if (vmc_all.ground_num >= 3)
						robotwb.exp_force_b[i].z += Mw * gw*vmc_robot_p.mess_scale / 2;//重力前馈
					else
						robotwb.exp_force_b[i].z += Mw * gw*vmc_robot_p.mess_scale;//重力前馈
				}

				//Rolr------------------------------------------
				if (vmc_all.ground_num >= 3)
					robotwb.exp_torque_i.x += limitw(robotwb.exp_att.roll - robotwb.now_att.roll, -25, 25) / 57.3*vmc_robot_p.att_rol.ki*dt;
				else if (vmc_all.ground_num <= 1)
					robotwb.exp_torque_i.x = 0;

				robotwb.exp_torque_i.x = limitw(robotwb.exp_torque_i.x, -robotwb.max_torque.x*0.25, robotwb.max_torque.x*0.25);

				robotwb.exp_torque.x = att_cmd[ROLr] = limitw(robotwb.exp_att.roll - robotwb.now_att.roll, -25, 25) / 57.3*vmc_robot_p.att_rol.kp
					- robotwb.now_rate.roll / 57.3*vmc_robot_p.att_rol.kd + robotwb.exp_torque_i.x;
				robotwb.exp_torque.x = limitw(robotwb.exp_torque.x, -robotwb.max_torque.x, robotwb.max_torque.x);

				//printf("exp_z=%f %f\n", fabs(robotwb.exp_pos_n_b[F].z), vmc_all.pos_n_b[F].z);
				force_dis_n_side();
			}
		}
		else
		{//清除积分
			robotwb.exp_torque_i.x = robotwb.exp_torque_i.y = robotwb.exp_torque_i.z = 0;
			robotwb.exp_force_i.x = robotwb.exp_force_i.y = robotwb.exp_force_i.z = 0;

			robotwb.exp_force.x = robotwb.exp_force.y = robotwb.exp_force.z = 0;
			robotwb.exp_torque.x = robotwb.exp_torque.y = robotwb.exp_torque.z = 0;
		}
		robotwb.exp_att.yaw = robotwb.now_att.yaw;
		break;

	default://清除积分
		robotwb.exp_force.x = robotwb.exp_force.y = robotwb.exp_force.z = 0;
		robotwb.exp_torque.x = robotwb.exp_torque.y = robotwb.exp_torque.z = 0;
		robotwb.exp_torque_i.x = robotwb.exp_torque_i.y = robotwb.exp_torque_i.z = 0;
		robotwb.exp_force_i.x = robotwb.exp_force_i.y = robotwb.exp_force_i.z = 0;
		for (i = 0; i < 4; i++)
			robotwb.Leg[i].tar_force_dis_n_reg.x = robotwb.Leg[i].tar_force_dis_n_reg.y = robotwb.Leg[i].tar_force_dis_n_reg.z = 0;
		break;
	}

	robotwb.now_rate_reg = robotwb.now_rate;
	spdx_reg = vmc_all.spd_ng.x;
	spdy_reg = vmc_all.spd_ng.y;
}


float GRF_OUT_FLT = 1.0;//足底力期望滤波
char MODE_ST[7] = { 0,0,0,0,0,0,0 };//Pitch Roll Nag Scale
char MODE_TORT[7] = { 0,0,0,0,0,0,0 };//Pitch Roll Nag Scale
#if USE_FORCE_REAL_GROUND
char CAL_MODE[4] = { 0,2,0,0 };//0 0均能
#else
char CAL_MODE[4] = { 0,0,0,0 };//0 0均能
#endif
//2_slove:: 0融合 1SLIP（姿态最优） 2全维（无法转向） 3最小二乘
//4_slove:: 0融合 1NC               2全维（无法转向） 3最小二乘no
double Div_Roll_ST = 0.5;//权重下横滚控制不住
double Div_Roll_TORT = 0.5;//权重下横滚控制不住
float flt_t_dt = 20;//能解决着地腿抖
void force_dis_n(void)//-------------------力分配
{
	// %     逆+（从箭头往里看）
	// %      z   y 逆+
	// %      |  /
	// %      | /
	// %      |/
	// %      o—————x 逆+
	// %1FL0   2FR1
	// %
	// %4BL3   3BR2
	static float yaw_control_mask = 0;
	int id_swap[4] = { 0,1,2,3}, i = 0;
	double FALL[3] = { 0 }, TALL[3] = { 0 }, Fp[2] = { 0 }, PC[3] = { 0 }, P1[3] = { 0 }, P2[3] = { 0 }, P3[3] = { 0 }, P4[3] = { 0 };
	char G[4] = { 0 };
	char G_NUM = 0;
	double ATT_NOW[3] = { 0,0,0 };
	//	#endif
	double F1c[3] = { 0 }, F2c[3] = { 0 }, F3c[3] = { 0 }, F4c[3] = { 0 };
	FALL[Xrw] = robotwb.exp_force.y;
	FALL[Yrw] = robotwb.exp_force.x;
	FALL[Zrw] = robotwb.exp_force.z;
	TALL[Xrw] = robotwb.exp_torque.y;
	TALL[Yrw] = robotwb.exp_torque.x;
	TALL[Zrw] = robotwb.exp_torque.z;

	// %1FL0   2FR1
	// %
	// %4BL3   3BR2
#if USE_FORCE_REAL_GROUND//力分配结果最终输出 最终按实际着地情况
#if  0
	G[0] = stand_force_enable_flag[id_swap[0]];
	G[1] = stand_force_enable_flag[id_swap[1]];
	G[2] = stand_force_enable_flag[id_swap[2]];
	G[3] = stand_force_enable_flag[id_swap[3]];
#else
	G[0] = robotwb.Leg[id_swap[0]].is_ground;
	G[1] = robotwb.Leg[id_swap[1]].is_ground;
	G[2] = robotwb.Leg[id_swap[2]].is_ground;
	G[3] = robotwb.Leg[id_swap[3]].is_ground;
	// printf("%d %d %d %d\n",G[0],G[1],G[2],G[3]);
	G_NUM = G[0] + G[1] + G[2] + G[3];

	if (vmc_all.gait_mode == TROT && 1) {
		if (G_NUM == 4)//delay
		{
			FALL[Xrw] = gain_tort[Xrw] * robotwb.exp_force.y;
			FALL[Yrw] = gain_tort[Yrw] * robotwb.exp_force.x;
			FALL[Zrw] = gain_tort[Zrw] * robotwb.exp_force.z;
			TALL[Xrw] = gain_tort[Xrw] * robotwb.exp_torque.y;
			TALL[Yrw] = gain_tort[Yrw] * robotwb.exp_torque.x;
			TALL[Zrw] = gain_tort[Zrw] * robotwb.exp_torque.z;
			CAL_MODE[3] = 0;
		}
		else {
			FALL[Xrw] = robotwb.exp_force.y;
			FALL[Yrw] = robotwb.exp_force.x;
			FALL[Zrw] = robotwb.exp_force.z;
			TALL[Xrw] = robotwb.exp_torque.y;
			TALL[Yrw] = robotwb.exp_torque.x;
			TALL[Zrw] = robotwb.exp_torque.z;
		}
	}
#endif
#else
	G[0] = robotwb.Leg[id_swap[0]].is_ground;
	G[1] = robotwb.Leg[id_swap[1]].is_ground;
	G[2] = robotwb.Leg[id_swap[2]].is_ground;
	G[3] = robotwb.Leg[id_swap[3]].is_ground;
	G_NUM = G[0] + G[1] + G[2] + G[3];

	if (vmc_all.gait_mode == TROT && 1) {

		if (G_NUM == 4)//delay
		{
			FALL[Xrw] = robotwb.exp_force.y;
			FALL[Yrw] = robotwb.exp_force.x;
			FALL[Zrw] = robotwb.exp_force.z;
			TALL[Xrw] = robotwb.exp_torque.y;
			TALL[Yrw] = robotwb.exp_torque.x;
			TALL[Zrw] = robotwb.exp_torque.z;
		}
		else {
			//修正落足
			FALL[Xrw] = gain_tort[Xrw] * robotwb.exp_force.y;
			FALL[Yrw] = gain_tort[Yrw] * robotwb.exp_force.x;
			FALL[Zrw] = gain_tort[Zrw] * robotwb.exp_force.z;
			TALL[Xrw] = gain_tort[Xrw] * robotwb.exp_torque.y;
			TALL[Yrw] = gain_tort[Yrw] * robotwb.exp_torque.x;
			TALL[Zrw] = gain_tort[Zrw] * robotwb.exp_torque.z;
		}
		G[0] = G[1] = G[2] = G[3] = 1;
	}
#endif

	if (vmc_all.param.leg_dof == 2) {
		Fp[0] = FALL[Yrw] - TALL[Zrw] / Www * !USE_FORCE_REAL_GROUND;//航向控制
		Fp[1] = FALL[Yrw] + TALL[Zrw] / Www * !USE_FORCE_REAL_GROUND;
		TALL[Zrw] *= 1 * 0.5;
	}

	PC[Xrw] = 0;
	PC[Yrw] = 0;
	PC[Zrw] = 0;

	P1[Xrw] = robotwb.Leg[id_swap[0]].epos_n.y;
	P1[Yrw] = robotwb.Leg[id_swap[0]].epos_n.x;
	P1[Zrw] = robotwb.Leg[id_swap[0]].epos_n.z;

	P2[Xrw] = robotwb.Leg[id_swap[1]].epos_n.y;
	P2[Yrw] = robotwb.Leg[id_swap[1]].epos_n.x;
	P2[Zrw] = robotwb.Leg[id_swap[1]].epos_n.z;

	P3[Xrw] = robotwb.Leg[id_swap[2]].epos_n.y;
	P3[Yrw] = robotwb.Leg[id_swap[2]].epos_n.x;
	P3[Zrw] = robotwb.Leg[id_swap[2]].epos_n.z;

	P4[Xrw] = robotwb.Leg[id_swap[3]].epos_n.y;
	P4[Yrw] = robotwb.Leg[id_swap[3]].epos_n.x;
	P4[Zrw] = robotwb.Leg[id_swap[3]].epos_n.z;
#if !USE_QP
	if (vmc_all.gait_mode == TROT && 1) {
		force_dis_8new(FALL, TALL, Fp, PC, P1, P2, P3,
			P4, G, ATT_NOW, Div_Roll_TORT, MODE_TORT, CAL_MODE, F1c, F2c, F3c, F4c);
	}
	else {//站立
		force_dis_8new(FALL, TALL, Fp, PC, P1, P2, P3,
			P4, G, ATT_NOW, Div_Roll_ST, MODE_ST, CAL_MODE, F1c, F2c, F3c, F4c);
	}
#endif
	float temp_min_z = 3;//
	float load_force_z = pos_force_p.load_fz;
#if USE_QP
#if !USE_MPC||MPC_GAIT_QP_FORCE||USE_MPC_QP
#if USE_MPC_QP
	if(vmc_all.gait_mode==STAND_RC)
		QP_Dis_Force(load_force_z, robotwb.max_force.z);
#else
	QP_Dis_Force(load_force_z, robotwb.max_force.z);
#endif
#endif
	//    for(i=0;i<4;i++)
	//        robotwb.Leg[i].tar_force_dis_n=robotwb.Leg[i].tar_force_dis_n_qp;
#if USE_MPC&&!MPC_GAIT_QP_FORCE
	F1c[Yrw] = robotwb.Leg[0].tar_force_dis_n_mpc.x;
	F1c[Xrw] = robotwb.Leg[0].tar_force_dis_n_mpc.y;
	F1c[Zrw] = robotwb.Leg[0].tar_force_dis_n_mpc.z;
	F2c[Yrw] = robotwb.Leg[1].tar_force_dis_n_mpc.x;
	F2c[Xrw] = robotwb.Leg[1].tar_force_dis_n_mpc.y;
	F2c[Zrw] = robotwb.Leg[1].tar_force_dis_n_mpc.z;
#else
	if(USE_MPC_QP&&vmc_all.gait_mode!=STAND_RC)
	{
		F1c[Yrw] = robotwb.Leg[id_swap[0]].tar_force_dis_n_mpc.x;
		F1c[Xrw] = robotwb.Leg[id_swap[0]].tar_force_dis_n_mpc.y;
		F1c[Zrw] = robotwb.Leg[id_swap[0]].tar_force_dis_n_mpc.z;
		F2c[Yrw] = robotwb.Leg[id_swap[1]].tar_force_dis_n_mpc.x;
		F2c[Xrw] = robotwb.Leg[id_swap[1]].tar_force_dis_n_mpc.y;
		F2c[Zrw] = robotwb.Leg[id_swap[1]].tar_force_dis_n_mpc.z;
		F3c[Yrw] = robotwb.Leg[id_swap[2]].tar_force_dis_n_mpc.x;
		F3c[Xrw] = robotwb.Leg[id_swap[2]].tar_force_dis_n_mpc.y;
		F3c[Zrw] = robotwb.Leg[id_swap[2]].tar_force_dis_n_mpc.z;
		F4c[Yrw] = robotwb.Leg[id_swap[3]].tar_force_dis_n_mpc.x;
		F4c[Xrw] = robotwb.Leg[id_swap[3]].tar_force_dis_n_mpc.y;
		F4c[Zrw] = robotwb.Leg[id_swap[3]].tar_force_dis_n_mpc.z;
	}
	else {
		F1c[Yrw] = robotwb.Leg[id_swap[0]].tar_force_dis_n_qp.x;
		F1c[Xrw] = robotwb.Leg[id_swap[0]].tar_force_dis_n_qp.y;
		F1c[Zrw] = robotwb.Leg[id_swap[0]].tar_force_dis_n_qp.z;
		F2c[Yrw] = robotwb.Leg[id_swap[1]].tar_force_dis_n_qp.x;
		F2c[Xrw] = robotwb.Leg[id_swap[1]].tar_force_dis_n_qp.y;
		F2c[Zrw] = robotwb.Leg[id_swap[1]].tar_force_dis_n_qp.z;
		F3c[Yrw] = robotwb.Leg[id_swap[2]].tar_force_dis_n_qp.x;
		F3c[Xrw] = robotwb.Leg[id_swap[2]].tar_force_dis_n_qp.y;
		F3c[Zrw] = robotwb.Leg[id_swap[2]].tar_force_dis_n_qp.z;
		F4c[Yrw] = robotwb.Leg[id_swap[3]].tar_force_dis_n_qp.x;
		F4c[Xrw] = robotwb.Leg[id_swap[3]].tar_force_dis_n_qp.y;
		F4c[Zrw] = robotwb.Leg[id_swap[3]].tar_force_dis_n_qp.z;
	}
#endif
#endif

	if (vmc_all.gait_mode == TROT||vmc_all.gait_mode== G_ETL || vmc_all.gait_mode == WALK) {

		G[0] = robotwb.Leg[0].is_ground*stand_force_enable_flag[4];
		G[1] = robotwb.Leg[1].is_ground*stand_force_enable_flag[4];

		robotwb.Leg[id_swap[0]].tar_force_dis_n.x = limitw(F1c[Yrw] * G[0], -robotwb.max_force.x, robotwb.max_force.x);
		robotwb.Leg[id_swap[0]].tar_force_dis_n.y = F1c[Xrw] * G[0];
		robotwb.Leg[id_swap[0]].tar_force_dis_n.z = limitw(F1c[Zrw] * G[0], temp_min_z, robotwb.max_force.z);
		if (vmc[id_swap[0]].param.trig_state == 4 && EN_TORT_LOAD_FORCE4) {//等待时load力
			robotwb.Leg[id_swap[0]].tar_force_dis_n.x = 0;
			robotwb.Leg[id_swap[0]].tar_force_dis_n.y = 0;
			robotwb.Leg[id_swap[0]].tar_force_dis_n.z = load_force_z;
		}
		else if (vmc[id_swap[0]].param.trig_state == 5 && EN_TORT_LOAD_FORCE5) {//等待时load力
			robotwb.Leg[id_swap[0]].tar_force_dis_n.x = 0;
			robotwb.Leg[id_swap[0]].tar_force_dis_n.y = 0;
			robotwb.Leg[id_swap[0]].tar_force_dis_n.z = load_force_z;
		}

		robotwb.Leg[id_swap[1]].tar_force_dis_n.x = limitw(F2c[Yrw] * G[1], -robotwb.max_force.x, robotwb.max_force.x);
		robotwb.Leg[id_swap[1]].tar_force_dis_n.y = F2c[Xrw] * G[1];
		robotwb.Leg[id_swap[1]].tar_force_dis_n.z = limitw(F2c[Zrw] * G[1], temp_min_z, robotwb.max_force.z);
		if (vmc[id_swap[1]].param.trig_state == 4 && EN_TORT_LOAD_FORCE4) {//等待时load力
			robotwb.Leg[id_swap[1]].tar_force_dis_n.x = 0;
			robotwb.Leg[id_swap[1]].tar_force_dis_n.y = 0;
			robotwb.Leg[id_swap[1]].tar_force_dis_n.z = load_force_z;
		}
		else if (vmc[id_swap[1]].param.trig_state == 5 && EN_TORT_LOAD_FORCE5) {//等待时load力
			robotwb.Leg[id_swap[1]].tar_force_dis_n.x = 0;
			robotwb.Leg[id_swap[1]].tar_force_dis_n.y = 0;
			robotwb.Leg[id_swap[1]].tar_force_dis_n.z = load_force_z;
		}
	}
	else//站立  good-------------------------------------------------------------------------------------------
	{
#if 1//力分配结果最终输出 最终按实际着地情况
		G[0] = robotwb.Leg[id_swap[0]].is_ground*stand_force_enable_flag[4];
		G[1] = robotwb.Leg[id_swap[1]].is_ground*stand_force_enable_flag[4];
#endif
		//printf("%f %f %f %f\n", F1c[Yrw] * G[0], F2c[Yrw] * G[1], F3c[Yrw] * G[2], F4c[Yrw] * G[3]);
		robotwb.Leg[id_swap[0]].tar_force_dis_n.x = limitw(F1c[Yrw] * G[0], -robotwb.max_force.x, robotwb.max_force.x);
		robotwb.Leg[id_swap[0]].tar_force_dis_n.y = F1c[Xrw] * G[0];
		robotwb.Leg[id_swap[0]].tar_force_dis_n.z = limitw(F1c[Zrw] * G[0], temp_min_z, robotwb.max_force.z);//站立最小为load力

		robotwb.Leg[id_swap[1]].tar_force_dis_n.x = limitw(F2c[Yrw] * G[1], -robotwb.max_force.x, robotwb.max_force.x);
		robotwb.Leg[id_swap[1]].tar_force_dis_n.y = F2c[Xrw] * G[1];
		robotwb.Leg[id_swap[1]].tar_force_dis_n.z = limitw(F2c[Zrw] * G[1], temp_min_z, robotwb.max_force.z);
	}
	//printf("%f %f\n", robotwb.Leg[0].tar_force_dis_n.x, robotwb.max_force.x);
	if (vmc_all.gait_mode == WALK&&0)
	{
		for (i = 0; i < 4; i++)
			robotwb.Leg[i].tar_force_dis_n.z += gait.imp_force[i][Zr];
	}
	//-----------------------滤波
	static char g_reg[4] = { 0 };
	static char ff_reg[4] = { 0 };
	static float flt_w[4] = { 0 };
#if 1
	for (i = 0; i < 2; i++) {
		if (vmc_all.gait_mode == TROT || vmc_all.gait_mode == G_ETL || vmc_all.gait_mode == WALK) {
			if (G[i] == 0) {//离地后滤波器复位WS
				robotwb.Leg[i].tar_force_dis_n_reg.x = 0;
				robotwb.Leg[i].tar_force_dis_n_reg.y = 0;
				robotwb.Leg[i].tar_force_dis_n_reg.z = 0;
				//robotwb.Leg[i].tar_force_dis_n_reg.= robotwb.Leg[i].tar_force_dis_n;
				flt_w[i] = 0;
			}
			else {
				flt_w[i] += flt_t_dt * 4 * 0.005;
				//flt_w[i]=GRF_OUT_FLT;
				flt_w[i] = limitw(flt_w[i], 0.2, GRF_OUT_FLT);//解决抖腿

				robotwb.Leg[i].tar_force_dis_n.x = robotwb.Leg[i].tar_force_dis_n.x*flt_w[i] + robotwb.Leg[i].tar_force_dis_n_reg.x*(1 - flt_w[i]);
				robotwb.Leg[i].tar_force_dis_n.y = robotwb.Leg[i].tar_force_dis_n.y*flt_w[i] + robotwb.Leg[i].tar_force_dis_n_reg.y*(1 - flt_w[i]);
				robotwb.Leg[i].tar_force_dis_n.z = robotwb.Leg[i].tar_force_dis_n.z*flt_w[i] + robotwb.Leg[i].tar_force_dis_n_reg.z*(1 - flt_w[i]);
				robotwb.Leg[i].tar_force_dis_n_reg = robotwb.Leg[i].tar_force_dis_n;

				//printf("f %f[%f] %f[%f] %f[%f] %f[%f]\n",
				//	robotwb.Leg[0].tar_force_dis_n.z, flt_w[0],
				//	robotwb.Leg[1].tar_force_dis_n.z, flt_w[1],
				//	robotwb.Leg[2].tar_force_dis_n.z, flt_w[2],
				//	robotwb.Leg[3].tar_force_dis_n.z, flt_w[3]);
			}
		}
		else {//STAND
			if (G[i] == 0) {//离地后滤波器复位WS
				robotwb.Leg[i].tar_force_dis_n_reg.x = 0;
				robotwb.Leg[i].tar_force_dis_n_reg.y = 0;
				robotwb.Leg[i].tar_force_dis_n_reg.z = 0;
				//robotwb.Leg[i].tar_force_dis_n_reg.= robotwb.Leg[i].tar_force_dis_n;
				flt_w[i] = 0;
			}
			else
			{
				flt_w[i] += flt_t_dt * 0.005;
				flt_w[i] = limitw(flt_w[i], 0.15, GRF_OUT_FLT);//解决抖腿

				robotwb.Leg[i].tar_force_dis_n.x = robotwb.Leg[i].tar_force_dis_n.x*flt_w[i] + robotwb.Leg[i].tar_force_dis_n_reg.x*(1 - flt_w[i]);
				robotwb.Leg[i].tar_force_dis_n.y = robotwb.Leg[i].tar_force_dis_n.y*flt_w[i] + robotwb.Leg[i].tar_force_dis_n_reg.y*(1 - flt_w[i]);
				robotwb.Leg[i].tar_force_dis_n.z = robotwb.Leg[i].tar_force_dis_n.z*flt_w[i] + robotwb.Leg[i].tar_force_dis_n_reg.z*(1 - flt_w[i]);
				robotwb.Leg[i].tar_force_dis_n_reg = robotwb.Leg[i].tar_force_dis_n;
			}
		}
	}


	//printf("%f\n", robotwb.Leg[0].tar_force_dis_n.x);
	for (i = 0; i < 2; i++) {
		g_reg[i] = vmc[i].ground;
		ff_reg[i] = stand_force_enable_flag[i];
	}
#endif

#if !USE_QP||MPC_GAIT_QP_FORCE
	//-----------------------摩擦圆锥
	float force_z_mu_use = 0;
	for (i = 0; i < 4; i++) {
#if 1
		force_z_mu_use = robotwb.Leg[i].tar_force_dis_n.z;//使用设定 good 一点
#else
		force_z_mu_use = robotwb.Leg[i].force_est_n_output.z;//使用估计
#endif
#if 0//带地形
		robotwb.Leg[i].tar_force_dis_n.x = limitw(
			robotwb.Leg[i].tar_force_dis_n.x,
			-force_z_mu_use * 0.707*vmc_robot_p.ground_mu*(cosdw(fabs(vmc_all.ground_att_cmd[PITr] * 2))),
			force_z_mu_use*0.707*vmc_robot_p.ground_mu*(cosdw(fabs(vmc_all.ground_att_cmd[PITr] * 2))));

		robotwb.Leg[i].tar_force_dis_n.y = limitw(
			robotwb.Leg[i].tar_force_dis_n.y,
			-force_z_mu_use * 0.707*vmc_robot_p.ground_mu*(cosdw(fabs(vmc_all.ground_att_cmd[ROLr] * 2))),
			force_z_mu_use*0.707*vmc_robot_p.ground_mu*(cosdw(fabs(vmc_all.ground_att_cmd[ROLr] * 2))));
#else
		robotwb.Leg[i].tar_force_dis_n.x = limitw(
			robotwb.Leg[i].tar_force_dis_n.x,
			-force_z_mu_use * 0.707*vmc_robot_p.ground_mu,
			force_z_mu_use*0.707*vmc_robot_p.ground_mu);

		robotwb.Leg[i].tar_force_dis_n.y = limitw(
			robotwb.Leg[i].tar_force_dis_n.y,
			-force_z_mu_use * 0.707*vmc_robot_p.ground_mu,
			force_z_mu_use*0.707*vmc_robot_p.ground_mu);
		//printf("leg=%d fx=%f fz=%f\n",i,robotwb.Leg[i].tar_force_dis_n.x ,robotwb.Leg[i].tar_force_dis_n.z);
#endif
	}
#endif

}

void force_dis_n_side(void) {
	 

}


void force_dis_n_side_LR(void) {
}


void reset_servo_interge(void)
{
	robotwb.exp_force.x = robotwb.exp_force.y = robotwb.exp_force.z = 0;
	robotwb.exp_torque.x = robotwb.exp_torque.y = robotwb.exp_torque.z = 0;
	robotwb.exp_torque_i.x = robotwb.exp_torque_i.y = robotwb.exp_torque_i.z = 0;
	robotwb.exp_force_i.x = robotwb.exp_force_i.y = robotwb.exp_force_i.z = 0;
}
